/**********************************************************************************
测试环境：
	   硬件：Windows11系统PC机。 Mega2560主控板，Mega2560扩展版
	   软件：Arduino IDE2.3.3

简介：实现Mega2560扩展版驱动电机标签1，2和3的pid调速.

日期：2024.11.26
QQ交流群：811348489
寄语：代码中能遇见你，真好，有你，真好....有你同创智能机器人科技(北京)有限公司.
**********************************************************************************/

#include "uni_motor.h" //调用编码电机库
#define RATE 115200    //定义串口波特率

/*--------------------------------------------------------------------------------------------------------------
功能：构造函数定义1. 用来声明编码电机对象.
概述：该构造函数为使用本实验提供的编码电机，方便开发者使用
函数原型：UNI_MOTOR(int motor_label, bool revs);
函数讲解：
    【显示的形式参数】：
      int motor_label： 表示电机标签
          如果motor_label=1:表示使用的编码电机引脚为：电机方向1:34   电机方向2:35   电机pwm:12  编码器A:18  编码器B:31
          如果motor_label=2:表示使用的编码电机引脚为：电机方向1:37   电机方向2:36   电机pwm:8   编码器A:19  编码器B:38
          如果motor_label=3:表示使用的编码电机引脚为：电机方向1:43   电机方向2:42   电机pwm:9   编码器A:3   编码器B:49
          如果motor_label=4:表示使用的编码电机引脚为：电机方向1:A4   电机方向2:A5   电机pwm:5   编码器A:2   编码器B:A1
      bool revs:表示控制编码电机正反转标志. true:表示电机正转编码器取正 false：表示电机正转编码器取反
    【省略的参数】：
      float _motor_kp: 10.0
      float _motor_ki: 0.05
      float _motor_kd: 25.05
      float _motor_pulse: 1560 (本实验用的车轮转动一圈，编码器计数为1560)
      float _motor_radius: 0.04米 (本实验用的编码电机外轮直径为8厘米，故这里的半径为0.04米)  */

float set_target_vel = 0.1; //设定电机目标速度
UNI_MOTOR motor1(1, true);
UNI_MOTOR motor2(2, true);
UNI_MOTOR motor3(3, true);

void setup() {
  delay(2500);
  Serial.begin(RATE); //打开串口
  motor1.init(); //编码电机1初始化设置
  motor2.init(); //编码电机2初始化设置
  motor3.init(); //编码电机3初始化设置
}

void loop() {
  motor1.set_vel(-set_target_vel);  //设定电机标签1正转要到达的目标速度-0.1m/s
  motor2.set_vel(set_target_vel);   //设定电机标签2正转要到达的目标速度0.1m/s
  motor3.set_vel(-set_target_vel);  //设定电机标签3正转要到达的目标速度-0.1m/s
  motor1.update();                  //实时更新电机1的状态
  motor2.update();                  //实时更新电机2的状态
  motor3.update();                  //实时更新电机3的状态
  delay(10);                        //由于电机库中电机周期设置的时间为10毫秒，故这里的延时不可删除
                                    //如若想要修改此处延时，需修改库中电机周期

  /*
  motor1.set_vel(set_target_vel);   //设定电机标签1反转要到达的目标速度0.1m/s
  motor2.set_vel(-set_target_vel);  //设定电机标签2正转要到达的目标速度-0.1m/s
  motor3.set_vel(set_target_vel);   //设定电机标签3反转要到达的目标速度0.1m/s
  motor1.update();                  //实时更新电机1的状态
  motor2.update();                  //实时更新电机2的状态
  motor3.update();                  //实时更新电机3的状态
  delay(10);                        //由于电机库中电机周期设置的时间为10毫秒，故这里的延时不可删除
                                    //如若想要修改此处延时，需修改库中电机周期
  */                            
}



